import lejos.nxt.*;
import lejos.nxt.comm.*;
import java.io.*;

/**
 * Test of Java streams over USB.
 * Run the PC example, USBSend, to send data.
 * 
 * @author Lawrie Griffiths
 *
 */
public class Control {

    public void doCommand(String command, AI ai) throws Exception {
        //LCD.clear();
        //LCD.drawString("doCommand",0,0);
        //LCD.refresh();
        if(command.equals("forward")) {
            this.moveForward();
        } else if(command.equals("backward")) {
            this.moveBackward();
        } else if(command.equals("left")) {
            this.moveLeft();
        } else if(command.equals("right")) {
            this.moveRight();
        } else if(command.equals("stop")) {
            this.stop();
        } else if(command.equals("recycle")) {
            //this.recycle(true);
        } else if(command.equals("opengrip")) {
            this.grip(false);
        } else if(command.equals("closegrip")) {
            this.grip(true);
        } else {
            this.stop();
        }
        
    }
    
    public void moveForward() throws Exception {
        Motor.A.forward();
        Motor.B.forward();
        Thread.sleep(1000);
        this.stop();
    }

    private void moveBackward() throws Exception {
        Motor.A.backward();
        Motor.B.backward();
        Thread.sleep(1000);
        this.stop();
    }
	
    private void moveLeft() throws Exception {
        Motor.A.forward();
        Motor.B.backward();
        Thread.sleep(1000);
        this.stop();
    }

    private void moveRight() throws Exception {
        Motor.A.backward();
        Motor.B.forward();
        Thread.sleep(1000);
        this.stop();
    }

    private void stop() {
        Motor.A.stop();
        Motor.B.stop();
    }

    private void grip(boolean direction) {
        int degrees;
        if(direction) {
            degrees=-90;
        } else {
            degrees=90;
        }
        Motor.C.smoothAcceleration(true);
	Motor.C.rotate(degrees,false);
    }
}

